#!/usr/bin/env python
# license removed for brevity


import rospy
from magabot.msg import Actuation



##########################################################

class _Getch:
    """Gets a single character from standard input.  Does not echo to the
screen."""
    def __init__(self):
        try:
            self.impl = _GetchWindows()
        except ImportError:
            self.impl = _GetchUnix()

    def __call__(self): return self.impl()


class _GetchUnix:
    def __init__(self):
        import tty, sys

    def __call__(self):
        import sys, tty, termios
        fd = sys.stdin.fileno()
        old_settings = termios.tcgetattr(fd)
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch


class _GetchWindows:
    def __init__(self):
        import msvcrt

    def __call__(self):
        import msvcrt
        return msvcrt.getch()


getch = _Getch()

####################################################################


def talker():
    pub = rospy.Publisher('magabot/actuation', Actuation, queue_size=10)
    rospy.init_node('magabot_commander', anonymous=False)
    pub.publish(msg)

if __name__ == '__main__':
	msg = Actuation()
	msg.set_velocity = True
	msg.velocity[0] = 0
	msg.velocity[1] = 0
	msg.set_leds = False
	msg.red = 0
	msg.green = 0
	msg.blue = 0
	
	while True:
		c = getch()
		rospy.loginfo("inviato al robot il comando: " + c)
		
		if c == 'q':
			msg.velocity[0] = 0
			msg.velocity[1] = 0
			try:
				talker()
			except rospy.ROSInterruptException: pass
			break
		elif c == 'w':
			msg.velocity[0] = 10
			msg.velocity[1] = 10
		elif c == 's':
			msg.velocity[0] = -10
			msg.velocity[1] = -10
		elif c == 'd':
			msg.velocity[0] = 10
			msg.velocity[1] = -10
		elif c == 'a':
			msg.velocity[0] = -10
			msg.velocity[1] = 10
		else:
			msg.velocity[0] = 0
			msg.velocity[1] = 0
		try:
			talker()
		except rospy.ROSInterruptException: pass
